/**
 * @copyright (C) COPYRIGHT 2022 Fortiortech Shenzhen
 * @file      MotorControl.c
 * @author    Fortiortech  Appliction Team
 * @since     Create:2021-04-10
 * @date      Last modify:2022-07-14
 * @note      Last modify author is Marcel He
 * @brief
 */

#include <MyProject.h>
/* Private variables ----------------------------------------------------------------------------*/

MotStateType data motor_ctrl_state;
MotStaM McStaSet;

/**
 * @brief     电机控制状态机
 * @warning   电机的状态只能在电机状态控制中切换，禁止在其他地方切换电机状态
 * @date      2022-07-14
 */
void MC_Control(void)
{
    switch (motor_ctrl_state)
    {
    case ready:
        Motor_Ready();
        if (mcFaultSource != FaultNoSource)
        {
            motor_ctrl_state = fault;
        }
        else if ((mcCurOffset.OffsetFlag == 1) && (isCtrlPowOn == true)) /* isCtrlPowOn 在给定转速后置 1 */
        {
            motor_ctrl_state = init;
            mcCurOffset.OffsetFlag = 0;
            mcCurOffset.OffsetCount = 0;
        }
        else
        {
        }
        break;

    case init:
        if (mcFaultSource != FaultNoSource)
        {
            motor_ctrl_state = fault;
        }
        else if (isCtrlPowOn == false)
        {
            motor_ctrl_state = stop;
        }
        else
        {
            Motor_Init();
            motor_ctrl_state = start;
        }

        break;

    case start:
        Motor_Static_Open();
        motor_ctrl_state = run;
        break;

    case run:
        if (mcFaultSource != FaultNoSource)
        {
            motor_ctrl_state = fault;
        }
        else if (isCtrlPowOn == false)
        {
            motor_ctrl_state = stop;
        }
        else
        {
        }

        break;

    case stop:
        if (mcFaultSource != FaultNoSource)
        {
            motor_ctrl_state = fault;
        }
        else
        {
            MC_Stop();
        }
        break;

    case brake:
        if (mcFaultSource != FaultNoSource)
        {
            motor_ctrl_state = fault;
        }
        else if (foc_control.motor_state_count == 0)
        {
            MOE = 0;
            ClrBit(DRV_CR, FOCEN);
            motor_ctrl_state = ready;
        }
        break;

    case fault:
        if (mcFaultSource == FaultNoSource) // 保护清零后关闭输出，软件重新运行
        {
            motor_ctrl_state = ready;
        }
        else
        {
            ClrBit(DRV_CR, FOCEN); // FOC Disable
            MOE = 0;
        }
        break;

    default:
        motor_ctrl_state = ready;
        break;
    } /* END OF SWITCH */
} /* END OF FUNCATION */
